package src.bojan;

import sim.field.continuous.Continuous2D;
import sim.util.Bag;
import sim.util.Double2D;
import bojan.robots.simulation.Obstacle;

public class GridInitializer {

	public int height;
	public int width;
	public double discretization;

	public GridInitializer() {
		this(30, 30, 1.0);
	}

	public GridInitializer(int width, int height, double discretization) {
		super();
		this.height = height;
		this.width = width;
		this.discretization = discretization;
	}

	public Continuous2D unknown() {
		return populate(Unknown.class);
	}

	private Continuous2D populate(Class type) {
		return populate(type, width, height);
	}

	private Continuous2D populate(Class type, int popWidth, int popHeight) {
		Continuous2D a = new Continuous2D(discretization, width, height);
		try {
			for (int j = 0; j < popWidth; j++) {
				for (int i = 0; i < popHeight; i++) {
					a.setObjectLocation(type.newInstance(), new Double2D(
							j + 0.5, i + 0.5));
				}
			}
		} catch (Exception e) {
			throw new RuntimeException();
		}
		return a;
	}

	public Continuous2D justObstacles() {
		return populate(Obstacle.class);
	}

	public Continuous2D knownSquare() {
		Continuous2D g = populate(FreeSpace.class, width - 1, height - 1);
		obstacleRectangle(g, 1, 29, 1, 29);

		return g;
	}
	
//	public Continuous2D knownNotRectangular(	) {
//		Continuous2D g = knownSquare();
//		TerrainMap map = new TerrainMap(g);
//		for (int i = 28; i < 29; i++) {
//			Double2D position = position(i, 2 * 28 - i);
//			forceObstacle(g, map, position);
//		}
//		
//	}	
	public Continuous2D knownSquares() {
		Continuous2D g = knownSquare();
		
		obstacleRectangle(g, 10, 13, 15, 20);

		return g;
	}
	
	public Continuous2D obstacleRectangle(Continuous2D g , int higher, int lower, int left, int right) {
		
		TerrainMap map = new TerrainMap(g);
		for (int i = left; i < right + 1; i++) {
			Double2D position = position(i, higher);
			forceObstacle(g, map, position);
			position = position(i, lower);
			forceObstacle(g, map, position);
		}
		for (int i = higher; i < lower + 1; i++) {
			Double2D 	position = position(left, i);
			forceObstacle(g, map, position);
			position = position(right, i);
			forceObstacle(g, map, position);
		}
		return g;
	}

	private Double2D position(int x, int y) {
		return new Double2D(x + 0.5, y + .5);
	}

	private void forceObstacle(Continuous2D g, TerrainMap map, Double2D position) {
		g.removeObjectsAtLocation(g.discretize(position));
		map.mapObstacle(position);
	}
}
